#include "vofa_basic.h"
#include <math.h>
#include "FOC_transform.h"
#include "PID.h"
#include "speed_close_loop.h"

#define M_PI 3.1415926535f

vofa_t vofa_handle;

// 定义控件
vofa_widget_t my_widget[3];
// 模拟控件设置数据

// 添加控件数据，用于回显确认，防止设置接收不同步，非必要

void vofa_basic_init(void)
{

    memset(&vofa_handle, 0, sizeof(vofa_t)); // 初始化句柄
    vofa_handle.init = vofa_interface_init;  // 挂载接口函数
    vofa_handle.send = vofa_interface_send;
    vofa_handle.recv = vofa_interface_recv;

    vofa_handle.protocol = VOFA_PROTOCOL_JUSTFLOAT; // 设置协议类型 也可以填 VOFA_PROTOCOL_FIREWATER

// 初始化控件
#define ADD_WIDGET(wv, name)                          \
    vofa_widget_init(&my_widget[wv], #name, &(name)); \
    vofa_add_widget(&vofa_handle, &my_widget[wv])
    ADD_WIDGET(0, pid_p.kp);
    ADD_WIDGET(1, pid_p.ki);
    ADD_WIDGET(2, position_t);

    vofa_init(&vofa_handle); // 初始化
}

void vofa_basic_loop(void)
{
    vofa_handle.recv_handle(&vofa_handle);
    vofa_send_samples(&vofa_handle);
}
